[r11]: / branches / PackMLState / Display_Escape_PLC / Plc Logic / Application / Function Blocks / Puzzle / BalastTankPuzzleStateMachine / svnobj  Maximize  Restore  History

Download this file

104 lines (102 with data), 25.3 kB 

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
IF NOT xInit THEN	
	UM				(		sName := 'Normal Operation', 
							dwAllowsLeavingFromStates := PACK_ML.State.Stopped OR PACK_ML.State.Aborted OR PACK_ML.State.Idle, 
		IF SysStrCmp(Client.sCommandToPLC, Client.LockCommands[0]) =  0 THEN		
			//First Lock Function
			;
		ELSIF SysStrCmp(Client.sCommandToPLC, 'UnSuspend') =  0 THEN		
			UnSuspend();
		
	END_CASE
	Client.iCommandToPLC := 0;
	Client.sCommandToPLC := '';
	Client.bReadyToPLC := FALSE;
CASE UM.CurrentState OF
PACK_ML.State.Stopped:
	;
PACK_ML.State.Aborted:
PACK_ML.State.Idle:
PACK_ML.State.Complete :	
PACK_ML.State.Execute :
    // NOTE: Remember that most math has the Y axis as positive above the X.
    //final double result = Math.toDegrees(Math.atan2(deltaY, deltaX)); 
    //return (result < 0) ? (360d + result) : result;
	actualPositionBoat := CMMT_ST_Rotery_Boot.ActualPosition;
	actualPositionLinear1 := CMMT_ST_Linear1.ActualPosition;
	deltaY := actualPositionLinear1 - 0;
	deltaX := actualPositionLinear2 - 500;
	angle := OSCAT_BASIC.ATAN2(deltaY,deltaX);
	targetPositionBoat := angle * 180 / 3.1415;		
	IF targetPositionBoat <= 0 THEN
		targetPositionBoat := 0;
	ELSIF targetPositionBoat >= 360 THEN
		targetPositionBoat := 360;
	CMMT_ST_Rotery_Boot.MoveAbsolute(Position:= targetPositionBoat , 
				Velocity:= 10000, 
				Acceleration:= 100000, 
				Deceleration:= 100000, 
				Jerk:= 10000000, 
				BufferMode:= MC_BUFFER_MODE.mcAborting, 
				ErrorID=> , 
				ErrorString=> );
	CASE Client.ActualStep OF		
		0:	
			CASE Client.Mode OF
				Mode.Mode1:
					CASE Client.Level OF
						Level.Beginner:	
							speed := 100;						
						Level.Placeholder1:
							speed := 200;
						Level.Intermediate:
							speed := 300;
						Level.Placeholder2:
							speed := 400;
						Level.Advanced:
							speed := 500;
					END_CASE
					Client.ActualStep := 100;
					executeBoat := TRUE;
				Mode.Mode2:					
					Client.ActualStep := 200;
				Mode.Mode3:					
					Client.ActualStep := 300;				 	
			END_CASE		
			bMove := CMMT_ST_Linear1.MoveAbsolute(Position:= 50 , 
				Acceleration:= 500, 
				Jerk:= 1000, 
			IF bMove THEN
			END_IF
			IF CMMT_ST_Linear1.TargetReached THEN
		102:
		103:
			END_IF		
		300:
	END_CASE	
	CMMT_ST_Rotery_Boot_MoveAbsolute.Execute:=FALSE;	
	CMMT_ST_Linear1.ResetHalt();
	StateComplete();		
			IF NOT CMMT_ST_Linear1.Enabled THEN
				Client.ActualStep := 200;
				Client.ActualStep := 10;				
			IF NOT CMMT_ST_Linear1.HomingValid THEN
			ELSIF NOT CMMT_ST_Rotery_Boot.HomingValid THEN
				Client.ActualStep := 300;				
			enable := CMMT_ST_Linear1.EnableDrive();
				Client.ActualStep := 0;
		110:
			home := CMMT_ST_Linear1.Home(Position := 0,HomingMethod := 0);
				Client.ActualStep := Client.ActualStep + 10;	
			IF CMMT_ST_Linear1.HomingValid THEN				
			enable := CMMT_ST_Rotery_Boot.EnableDrive();
		210:
			home := CMMT_ST_Rotery_Boot.Home(Position := 0,HomingMethod := 0);
			IF CMMT_ST_Rotery_Boot.HomingValid THEN				
PACK_ML.State.Completing :
PACK_ML.State.Aborting :
	CMMT_ST_Rotery_Boot.DisableDrive();
PACK_ML.State.Stopping:
		0:
			StopLinear.Execute := TRUE;
		10:
				StopLinear.Execute := FALSE;
				Client.ActualStep := 20;